/* Actual NTP implementation */

/* Copyright (C) 2009 David Zanetti
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with this program; if not, write to the Free Software Foundation, Inc.,
 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */


#include <stdio.h>
#include <avr/io.h>
#include "global.h"
#include "ether.h"
#include "ip.h"
#include <string.h>
#include "checksum.h"
#include "enc28j60.h"
#include <util/delay.h>
#include "serial.h"
#include <avr/pgmspace.h>
#include "udp.h"
#include "ntp.h"
#include <avr/interrupt.h>
#include <util/atomic.h>
#include "clock.h"

typedef struct {
		uint8_t li_ver_mode;
		uint8_t stratum;
		uint8_t poll;
		int8_t precision;
		uint8_t root_delay[4];
		uint8_t root_dispersion[4];
		uint8_t ref_id[4];
		uint8_t ref_time[8];
		uint8_t orig_time[8];
		uint8_t rx_time[8];
		uint8_t tx_time[8];
} ntp_t;

uint8_t ntp_process(uint8_t *pkt, uint8_t *tx_pkt) {
	ntp_t *in, *out;
	uint32_t rx_sec;
	uint16_t rx_ms, rx_us;

	/* save date and time of receive, so we can correctly report our
	 * processesing delay */
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		rx_us = TCNT1;
		rx_sec = rtc_s;
		rx_ms = clock_frac;
	}

	/* get the payload from the IP packet */
	in = (ntp_t *) udp_payload(pkt);

	// serial0_tx_PGM(PSTR("ntp: received packet\r\n"));

	// serial0_tx_PGM(PSTR("ntp: version "));
	// serial0_tx_dec((in->li_ver_mode & 0x38) >> 3);
	// serial0_tx_PGM(PSTR("\r\n"));

	// serial0_tx_PGM(PSTR("ntp: mode "));
	// serial0_tx_dec(in->li_ver_mode & 0x7);
	// serial0_tx_PGM(PSTR("\r\n"));

	/* start building a reply packet */
	udp_reply(pkt,tx_pkt);


	out = (ntp_t *) udp_payload(tx_pkt);

	/* set leap indication (don't know!), version, and mode */
	if (flag_clock & FLAG_CLOCK_SYNC) {
		out->li_ver_mode = 0x1C; // 0x24 = V4
	} else {
		out->li_ver_mode = 0xDC; /* li = 11, no sync */
	}

	if (flag_clock & FLAG_CLOCK_SYNC) {
		out->stratum = 1; /* we are a very low stratum server */
	} else {
		out->stratum = 16; /* unsynced */
	}
	out->precision = -10;
	out->poll = 6; /* you can hit us pretty hard */

	/* provide out reference clock delay. The GPS claims we get a pulse
    within 200ns, but by the time we process that.. so call it 1us */
	out->root_delay[0] = 0;
	out->root_delay[1] = 0;
	out->root_delay[2] = 0;
	out->root_delay[3] = 0x10;

	/* how much it drifts around by, say 10ms */
	out->root_dispersion[0] = 0;
	out->root_dispersion[1] = 0;
	out->root_dispersion[2] = 0;
	out->root_dispersion[3] = 0x40;

	if (flag_clock & FLAG_CLOCK_SYNC) {
		/* our reference clock id is "GPS" */
		out->ref_id[0] = 'G';
		out->ref_id[1] = 'P';
		out->ref_id[2] = 'S';
		out->ref_id[3] = 0;
	} else {
		out->ref_id[0] = 0;
		out->ref_id[1] = 0;
		out->ref_id[2] = 0;
		out->ref_id[3] = 0;
	}

	/* now the big difficult parts, converting our clock data to NTP format */
	{
		uint32_t ref_s;
		uint32_t ref_us;
		uint32_t ref_frac;

		/* ref seconds is easy */
		ref_s = rtc_update_s + 2208988800UL;

		/* ref fraction is hard */
		//  ref_us = ((clock_update_us / 20) + clock_update_ms * 1000);
		//  ref_frac = (ref_us * 1825) >> 5;
		//  ref_frac = ((ref_us << 12) + (ref_us << 8) - ref_frac);
		ref_frac = 0;

		/* now store it */
		out->ref_time[0] = (ref_s >> 24);
		out->ref_time[1] = (ref_s >> 16);
		out->ref_time[2] = (ref_s >> 8);
		out->ref_time[3] = (ref_s & 0xff);
		out->ref_time[4] = (ref_frac >> 24);
		out->ref_time[5] = (ref_frac >> 16);
		out->ref_time[6] = (ref_frac >> 8);
		out->ref_time[7] = (ref_frac & 0xff);
	}

	/* originate we can just copy as-is from incoming TX time */
	memcpy(&(out->orig_time),&(in->tx_time),8);

	/* RX time is when we got the packet, saved above */
	{
		uint32_t ref_s;
		uint32_t ref_us;
		uint32_t ref_frac;

		/* ref seconds is easy */
		ref_s = rx_sec + 2208988800UL;

		/* ref fraction is hard */
		ref_us = rx_us + (rx_ms * TICKLEN);
		//  ref_us = rx_us + (rx_ms * 7999UL);
		//  ref_frac = (ref_us * 1825) >> 5;
		//  ref_frac = ((ref_us << 12) + (ref_us << 8) - ref_frac);
		ref_frac = (uint32_t) ref_us * 430UL;
		// 215

		/* now store it */
		out->rx_time[0] = (ref_s >> 24);
		out->rx_time[1] = (ref_s >> 16);
		out->rx_time[2] = (ref_s >> 8);
		out->rx_time[3] = (ref_s & 0xff);
		out->rx_time[4] = (ref_frac >> 24);
		out->rx_time[5] = (ref_frac >> 16);
		out->rx_time[6] = (ref_frac >> 8);
		out->rx_time[7] = (ref_frac & 0xff);
	}

	/* and TX time is just now, overwrite the RX bits so we get a clean copy */
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		rx_sec = rtc_s;
		rx_ms = clock_frac;
		rx_us = TCNT1;
	}

	{
		uint32_t ref_s;
		uint32_t ref_us;
		uint32_t ref_frac;

		/* ref seconds is easy */
		ref_s = rx_sec + 2208988800UL;

		/* ref fraction is hard */
		ref_us = rx_us  + (rx_ms * TICKLEN);
		//  ref_us = rx_us  + (rx_ms * 7999UL);
		//  ref_frac = (ref_us * 1825) >> 5;
		//  ref_frac = ((ref_us << 12) + (ref_us << 8) - ref_frac);
		ref_frac = (uint32_t) ref_us * 430UL;
		// 215

		/* now store it */
		out->tx_time[0] = (ref_s >> 24);
		out->tx_time[1] = (ref_s >> 16);
		out->tx_time[2] = (ref_s >> 8);
		out->tx_time[3] = (ref_s & 0xff);
		out->tx_time[4] = (ref_frac >> 24);
		out->tx_time[5] = (ref_frac >> 16);
		out->tx_time[6] = (ref_frac >> 8);
		out->tx_time[7] = (ref_frac & 0xff);
	}

	/* and now send it */
	{
		uint16_t len;
		len = udp_reply_finish(pkt, tx_pkt, sizeof(ntp_t));
		len = ip_reply_finish(pkt, tx_pkt, len);
		//  if (!(enc28j60_tx(tx_pkt,len+14))) {
		//   serial0_tx_PGM(PSTR("ntp: failed to send, abort\r\n"));
		//  }
	}

	return 0;
} 



